#include "Numeric.h"

using namespace std;
#include <iostream> 

using namespace std;

//********start of junyangs code
function::function(){
}

double function::Df(double x, double h) {
	return (f(x+h)-f(x-h))/(2.0*h);
  }

double function::DDf(double x, double h) {
	return (f(x+h)-2.0*f(x)+f(x-h))/(h*h);
  }

Matrix function::fit_least_squares(const std::vector<std::vector<double> >& points,const std::vector<double (*)(double)>& f, double& chi2){
	Matrix A = Matrix(points.size(), f.size());            
	Matrix b = Matrix(points.size(), points.size());
	for (unsigned int i = 0; i < A.rows; ++i){
		double weight = points[i].size() > 2 ? 1.0/points[i][2] : 1.0;
		b(i,0) = weight*double(points[i][1]);
		for (unsigned int j = 0; j < A.cols; ++j) {
			A(i,j) = weight*f[j](points[i][0]);
		}
	}
	Matrix c = (1.0/(tranpose(A)*A))*(tranpose(A)*b);
	Matrix chi = A*c-b;
	chi2 = pow( norm(chi), 2);
	return c;
}

Matrix function::exp(const Matrix &A, double ap, double rp, double ns){
	    Matrix t = identity(A.cols);
        Matrix s = identity(A.cols);
	    for (int k=1; k<ns; k++){
		    t = t*A/k;
		    s = s + t;
		if( norm(t)< max(ap, norm(s)*rp)) return s;
	    }
		throw string("no convergence");
  }



double function::solve_bisection(double a, double b, double ap, double rp, double ns){
	if ( f(a) == 0 ) return a;
    if ( f(b) == 0 ) return b;
	if ( f(a)*f(b) > 0 )
		throw string("f(a) and f(b) must have opposite sign");
	for (int k=0; k<ns; k++){
		double x = (a+b)/2;
		double fx = f(x);
		double fa = f(a);
		double fb = f(b);
        if ( f(x)==0 || abs(x)<max(ap,abs(x)*rp)) return x;
		else if ( f(x)*f(a) < 0 ){
			b = x;
			fb = fx;
		}
		else {
			a = x;
			fa = fx;
		}
     }
	 throw string("no convergence");
  }  
   
double function::solve_newton(double x_guess, double ap, double rp, double ns) {
	double x_old, x = x_guess;
    for(int k=0; k<ns; k++) {
      x_old = x;
      x = x - f(x)/Df(x);
      if(abs(x-x_old)<max(ap,rp*abs(x))) return x;
    }
    throw string("no convergence");
  } 

double function::solve_newton_stabilized(double a, double b, double ap, double rp, double ns){
	double x = (a+b)/2;
	double x_old;
	double fx = f(x);
	double fa = f(a);
	double fb = f(b);
	if ( f(a) == 0 ) return a;
    if ( f(b) == 0 ) return b;
	if ( f(a)*f(b) > 0 )
		throw string("f(a) and f(b) must have opposite sign");
	for(int k=0; k<ns; k++) {
    x_old = x;
	if ( abs(Df(x)) > ap )  x = x - f(x)/Df(x);
	if ( x == x_old || x<a || x>b ) x = (a+b)/2;
	if ( f(x) == 0 || abs(x-x_old) < max(ap,abs(x)*rp) ) return x;
	if ( f(x)*f(a) < 0){
		b = x;
		fb = fx;
	  }
	else{
		a = x;
		fa = fx;
	  }
	}
    throw string("no convergence");
  }

double function::optimize_bisection(double a, double b, double ap, double rp, double ns){
	   
	   double x = (a+b)/2;
	   double fx = f(x);
	   double fa = f(a);
	   double fb = f(b);
	   double Dfa = Df(a);
	   double Dfb = Df(b);
	if ( Dfa == 0 ) return a;
	if ( Dfb == 0 ) return b;
	if ( Dfa*Dfb > 0 ) throw string("Df(a) and Df(b) must have opposite sign");
    for(int k=0; k<ns; k++) { 
	   if ( Df(x) == 0 || abs(b-a) < max( ap, abs(x)*rp ) ) return x;
	   else if ( Df(x) * Df(a) < 0 ){
		   b = x;
		   fb = fx;
	   }
	   else{
		   a = x;
		   fa = fx;
	   }
	}
    throw string("no convergence");
  }

double function::optimize_newton(double x_guess, double ap, double rp, double ns) {  
	double x_old, x = x_guess;
    for(int k=0; k<ns; k++) {
      x_old = x;
      x = x - Df(x)/DDf(x);
      if(abs(x-x_old)<max(ap,rp*abs(x))) return x;
    }
    throw string("no convergence");
  }  

void test_bisection(){
	//takes x defins f 
	class MyFunction : public function {
	double f(double x) { return (x-2)*(x-5); }
	};
	//produces func;
	MyFunction func;
	//tests func bisection for a~b = 1~3
	cout << func.solve_bisection(1.0,3.0)<<endl;
};

void test_newton(){
	//takes x defins f 
	class MyFunction : public function {
	double f(double x) { return (x-2)*(x-5); }
	};
	//produces func;
	MyFunction func;
	//tests func newton optimizer for x guess 1.0
	cout << func.solve_newton(1.0)<<endl;
};

void test_newton_stabilized(){
	//takes x defins f 
	class MyFunction : public function {
	double f(double x) { return (x-2)*(x-5); }
	};
	//produces func;
	MyFunction func;
	//tests func newton optimizer for a~b = 1~3
	cout << func.solve_newton_stabilized(1.0,3.0)<<endl;
};

void test_optimize_bisection(){
	//takes x defins f 
	class MyFunction : public function {
	double f(double x) { return (x-2)*(x-5); }
	};
	//produces func;
	MyFunction func;
	//tests func newton optimizer for a~b = 2~5
	cout << func.optimize_bisection(2.0,5.0)<<endl;
};

//tests newton optimizer for the function (x-2)(x-5) 
void test_optimize_newton(){
	//takes x defins f 
	class MyFunction : public function {
		double f(double x) { return (x-2)*(x-5); }
	};
	//produces func;
	MyFunction func;
	//tests func newton optimizer for x guess 3.0
	cout << func.optimize_newton(3.0)<<endl;
};




//********end of junyangs code

//***** John fitzgeralds code Start
class Function {
public:
  //virtual functions for f and g.  Take x.
  virtual double f(double x)=0;
  virtual double g(double x)=0;
  //takes x and a small h returns derivative of f
  double Df(double x, double h=0.00001) {
    return (f(x+h)-f(x-h))/(2.0*h);
  }
  //takes x and a small h returns second derivative of f
  double DDf(double x, double h=1e-5) {
    return (f(x+h)-2.0*f(x)+f(x-h))/(h*h);
  }
  //takes x and a small h returns derivative of g
  double Dg(double x, double h=0.00001) {
    return (f(x+h)-f(x-h))/(2.0*h);
  }
  //takes x and a small h returns second derivative of g
  double DDg(double x, double h=1e-5) {
    return (f(x+h)-2.0*f(x)+f(x-h))/(h*h);
  }
  //takes x, number of steps, absolute precision and relative precision and returns 0 via secant method
  double solve_secant(double x_guess, double ap=1e-6, double rp=1e-4, int ns=20) {
    double x_old, x = x_guess;
	double fx= f(x);
	double fxold=fx;
	double Dfx=Df(x);
    for(int k=0; k<ns; k++) {
	  if(abs(Dfx)<ap) throw string("unstable solution");
      x_old = x;
	  fxold=fx;
      x = x - f(x)/Df(x);
      if((k>2)&&(abs(x-x_old)<max(ap,rp*abs(x)))) return x;
	  fx=f(x);
	  Dfx=(fx-fxold)/(x-x_old);
	  
    }
    throw string("no convergence");
  }  
  //takes x, number of steps, absolute precision, relative precision and returns 0 via fixed point method
  double solve_fixed_point(double x_guess, double ap=1e-6, 
			double rp=1e-4, int ns=100) {
    double x_old, x = x_guess;
	double Dgx=Dg(x);
    for(int k=0; k<ns; k++) {
	  if( abs(Dg(x)>=1)) throw string("unstable solution");
      x_old = x;
      x = g(x);
	  
      if(abs(x-x_old)<max(ap,rp*abs(x))) return x;
    }
    throw string("no convergence");
  }

    //takes x, number of steps, absolute precision and relative precision and returns min/max of x via secant method
  double optimize_secant(double x_guess, double ap=1e-6, double rp=1e-4, int ns=100) {
    double x_old, x = x_guess;
	double fx= f(x);
	double fxold=fx;
	double Dfx=Df(x);
	double DDfx=DDf(x);
	double Dfxold=Dfx;
    for(int k=0; k<ns; k++) {
	  if(abs(DDfx)<ap) throw string("unstable solution");
      x_old = x;
	  Dfxold=Dfx;
      x = x - Df(x)/DDf(x);
      if((abs(x-x_old)<max(ap,rp*abs(x)))) return x;
	  fx=f(x);
	  Dfx=Df(x);
	  DDfx=(Dfx-Dfxold)/(x-x_old);
	  
    }
    throw string("no convergence");
}
  //takes a,b, precisions and steps returns min/max of f using newton stabilized
   double optimize_newton_stabilized(double a, double b, double ap=1e-6, double rp=1e-4, int ns=20) {
    double x=(a+b)/2.0;
	double Dfa=Df(a);
	double Dfb=Df(b);
	double Dfx=Df(x);
	if (Dfa==0) return a;
	if (Dfb==0) return b;
	if (Dfa*Dfb>0) throw string("Dfa and dfb must have opposite signs");
	
	double fx= f(x);
	double fxold=fx;
	double xold=x;
	
	double DDfx=DDf(x);
	double Dfxold;
    for(int k=0; k<ns; k++) {
	  if(Dfx==0) return x;
      xold = x;
	  fxold=fx;
	  Dfxold=Dfx;
	  if (abs(DDfx)>ap) x=x-Dfx/DDfx;
	  if((x==xold || (x<a)) || (x>b)) x=(a+b)/2;
	  if((abs(x-xold)<max(ap,rp*abs(x)))) return x;
	  fx=f(x);
	  Dfx=(fx-fxold)/(x-xold);
	  DDfx=(Dfx-Dfxold)/(x-xold);
	  if (Dfx * Dfa <0) {
		b=x;
		Dfb=Dfx;
	  }
	  else {
		  a=x;
		Dfa=Dfx;

	  }
    }
    throw string("no convergence");
}

 //takes a,b, precisions and steps returns min/max of f using optimize golden search
   double optimize_golden_search(double a, double b, double ap=1e-6, double rp=1e-4, int ns=100) {
    double tau=((sqrt(5.0))-1)/2.0;
	double x1=a+(1.0-tau)*(b-a);
	double x2=a+tau*(b-a);
	double fa=f(a);
	double f1=f(x1);
	double f2=f(x2);
	double fb=f(b);
    for(int k=0; k<ns; k++) {
	  if(f1>f2){
		a=x1;
		fa=f1;
		x1=x2;
		f1=f2;
		x2=a+tau*(b-a);
		f2=f(x2);
	  }
	  else {
		b=x2;
		fb=f2;
		x2=x1;
		f2=f1;
		x1=a+(1.0-tau)*(b-a);
		f1=f(x1);
	  }
	  if ((k>2)&&(abs(b-a)<max(ap,rp*abs(b)))) return b;
    }
    throw string("no convergence");
}
};

//tests fixed point solver for the function -.5x +1 and g(x)=x of .5x+1
void testfixedpoint(){
	//takes x defines virtual functions f and g
	class MyFunction : public Function {
	public:
	double f(double x) {return -.5*x+1;}
	double g(double x) {return .5*x+1;}
	};
	//produces func;
	MyFunction func;
  //tests fixed point
  cout << func.solve_fixed_point(0) <<endl;
};

//tests secant solver for the function -.5x +1 and g(x)=x of .5x+1
void testsecant(){
	//takes x defines virtual functions f and g
	class MyFunction : public Function {
	public:
	double f(double x) {return -.5*x+1;}
	double g(double x) {return .5*x+1;}
	};
	//produces func;
	MyFunction func;
  //tests fixed point
  cout << func.solve_fixed_point(0) <<endl;
};

//tests secant optimizer for the function (x-1)^2+1 and g(x)=x of x^2+x+1
void testsecantopt(){
	//takes x defins f and g
	class MyOtherFunction : public Function {
	double f(double x) { return (x-1)*(x-1)+1; }
	double g(double x) {return x*x+1+x;}
	};
	//produces func;
	MyOtherFunction func2;
	//tests func secant optimizer at 0
	cout << func2.optimize_secant(0)<<endl;
};

//tests secant optimizer for the function (x-1)^2+1 and g(x)=x of x^2+x+1
void testnewtonopt(){
	//takes x defins f and g
	class MyOtherFunction : public Function {
	double f(double x) { return (x-1)*(x-1)+1; }
	double g(double x) {return x*x+1+x;}
	};
	//produces func;
	MyOtherFunction func2;
	//tests func newton optimizer for a-b -1 to 4
	cout << func2.optimize_newton_stabilized(-1,4)<<endl;
};

//tests golden search optimizer for the function (x-1)^2+1 and g(x)=x of x^2+x+1
void testgoldensearch(){
	//takes x defins f and g
	class MyOtherFunction : public Function {
	double f(double x) { return (x-1)*(x-1)+1; }
	double g(double x) {return x*x+1+x;}
	};
	//produces func;
	MyOtherFunction func2;
	//tests func2 golden search optimizer from -1 to 4
	cout << func2.optimize_golden_search(-1,4)<<endl;
};
//checks if x is negative if so throws an exception.  Else returns square root.
double sqroot(double x){
	try
	{
		if(x<0)
			throw x;
		return sqrt( (x));
	}
	catch(double x)
	{
		cout<< "can't take the square root of a negative number"<<endl;
		return x;
	}

};
//tests sqroot for 1 and -1
void testsqroot(){
	cout << sqroot(4.0)<<endl;
	cout<<sqroot(-1.0)<<endl;
};

//***** John fitzgeralds code End





int main() {
  
  test_bisection();
  test_newton();
  test_newton_stabilized();
  test_optimize_bisection();
  test_optimize_newton();
  return 0;
  //test functions
matrixOperationsTest();
normTest();
symmetricZeroTest();
conditionNumTest();
choleskyTest();
positiveDefinite();
markowtizTest();

testfixedpoint();
  testsecant();
  testsecantopt();
  testnewtonopt();
  testgoldensearch();
  testsqroot();

system("Pause");
 }